#include <compass.h>


void vector_cross(const vector *a,const vector *b, vector *out)
{
  out->x = a->y*b->z - a->z*b->y;
  out->y = a->z*b->x - a->x*b->z;
  out->z = a->x*b->y - a->y*b->x;
}

float vector_dot(const vector *a,const vector *b)
{
  return a->x*b->x+a->y*b->y+a->z*b->z;
}
vector vector_sum(vector *a,vector *b)
{
	vector sum;
	sum.x=a->x+b->x;
	sum.y=a->y+b->y;
	sum.z=a->z+b->z;
	return sum;
}

void vector_normalize(vector *a)
{
  float mag = sqrt(vector_dot(a,a));
  a->x /= mag;
  a->y /= mag;
  a->z /= mag;
}


compass::compass()
{

}

void compass::read(void)
{
}

void compass::attitude(void)
{

vector temp_a = a;
vector_normalize(&temp_a);
vector temp_m = m;
vector_normalize(&temp_m);

//Picth
	pitch=asin(-temp_a.x);
//Roll
	roll=asin(temp_a.y/cos(pitch));
// compute E and N
    vector E;
    vector N;
    vector from={-1,0,0};	
    vector_cross(&temp_a,&temp_m,&E);
    vector_cross(&E,&temp_a,&N);
    yaw=fmod(2*M_PI-atan2(vector_dot(&E,&from), vector_dot(&N,&from)),2*M_PI);

    /*	
    yaw=atan2(vector_dot(&E,&from), vector_dot(&N,&from)),2*M_PI;
    if (yaw<0) {
	yaw=2*M_PI+yaw;
    }
    */

}
